import os

from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument,ExecuteProcess
from launch.actions import IncludeLaunchDescription
from launch.conditions import IfCondition
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution,PythonExpression

from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare   

def generate_launch_description():
    px4_path = LaunchConfiguration('px4_path')
    uav_pose = LaunchConfiguration('uav_pose')
    uav_id = LaunchConfiguration('uav_id')
    uav_num = LaunchConfiguration('uav_num')
    sim_model = LaunchConfiguration('sim_model')

    px4_path_launch_arg = DeclareLaunchArgument(
        'px4_path',
        default_value= '/home/hpdadi/px/PX4-Autopilot',
        description='PX4 Firmware Path'
    )

    uav_pose_launch_arg = DeclareLaunchArgument(
        'uav_pose',
        default_value= '0,0',
        description='UAV Pose'
    )

    uav_id_launch_arg = DeclareLaunchArgument(
        'uav_id',
        default_value= 'uav1',
        description='UAV ID'
    )

    uav_num_launch_arg = DeclareLaunchArgument(
        'uav_num',
        default_value= '0',
        description='UAV Number'
    )
    sim_model_launch_arg = DeclareLaunchArgument(
        'sim_model',
        default_value= 'gz_x500',
        description='Simulation Model'
    )

    #执行 PX4_GZ_STANDALONE=1 PX4_SYS_AUTOSTART=4001 PX4_SIM_MODEL=gz_x500 PX4_GZ_MODEL_POSE="2,3" PX4_UXRCE_DDS_NS=uav2 ./build/px4_sitl_default/bin/px4 -i 1
    
    px4_start = ExecuteProcess(
            cmd=[
                PathJoinSubstitution([px4_path, 'build', 'px4_sitl_default', 'bin', 'px4']),
                '-i',
                uav_num
            ],
            additional_env={
                'PX4_GZ_STANDALONE': '1',
                'PX4_SYS_AUTOSTART': '4001',
                'PX4_GZ_MODEL_NAME': sim_model,
                'PX4_GZ_MODEL_POSE': uav_pose,
                'PX4_UXRCE_DDS_NS': uav_id
            },
            cwd=px4_path,  # 根据实际情况设置工作目录
            output='screen'
        )
 

    return LaunchDescription([
        px4_path_launch_arg,
        uav_pose_launch_arg,
        uav_id_launch_arg,
        uav_num_launch_arg,
        sim_model_launch_arg,
        px4_start
    ])
